#include "FreeCamera.h"
#include "PortableGL.h"
#include <stdio.h>
#include "../geom/GeomMatrix.h"
#include "../algebra/MatrixOps.h"
#include <iostream>
using namespace glutil;

namespace glutil
{
	static const float FREECAMERA_NEAR_DISTANCE = 0.1;
	static const float FREECAMERA_FAR_DISTANCE = 1000;
}

void FreeCamera::move(geom::Vector3D distance)
{
	for(int i=0;i<3;++i)
		m_position[i]+= distance.dv[i];
}

void FreeCamera::lookAt(geom::Point3D pos)
{
	geom::Vector3D viewDir = geom::Vector3D(
			pos.x - m_position[0],
			pos.y - m_position[1],
			pos.z - m_position[2]).getNormalized();

	for(int i=0;i<3;++i)
		m_viewDir[i] = viewDir.dv[i];
}

void FreeCamera::advance(float amount)
{
	m_position += m_viewDir*amount;
}

void FreeCamera::strafeRight(float amount)
{
	algebra::Vector3f rightV= m_viewDir ^ m_up;
	m_position += rightV*amount;
}

void FreeCamera::strafeLeft(float amount)
{
	algebra::Vector3f leftV= m_up ^ m_viewDir;
	m_position += leftV*amount;
}

void FreeCamera::moveUp(float amount)
{
	algebra::Vector3f leftV= m_up ^ m_viewDir;
	algebra::Vector3f upV= leftV ^ m_viewDir;

	m_position+= upV * amount;
}

void FreeCamera::mouseLookTurn(float dx, float dy)
{
	algebra::Matrix4f rotMatrix;

	algebra::Vector4f viewVec;
	for(int i=0;i<3;++i)
		viewVec[i] = m_viewDir[i];
	viewVec[3] = 0;
	
	rotMatrix = geom::createRotationMatrixAroundY(dx);
	viewVec = rotMatrix * viewVec;

	viewVec[1] += dy;

	float module = sqrt(viewVec[0] * viewVec[0] + viewVec[1] * viewVec[1] + viewVec[2]*viewVec[2]);
	viewVec/=module;
	if(viewVec[1] > 0.95)
		viewVec[1] = 0.95;
	if(viewVec[1] < -0.95)
		viewVec[1] = -0.95;
			

	for(int i=0;i<3;++i)
		m_viewDir[i] = viewVec[i];
}

void FreeCamera::setAspectRatio(float aspectRatio)
{
	m_ratio = aspectRatio;
}

void FreeCamera::setAsGLCamera()
{
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective (m_FOV, m_ratio, FREECAMERA_NEAR_DISTANCE, FREECAMERA_FAR_DISTANCE);

	glMatrixMode(GL_MODELVIEW);                          // Make sure the MODELVIEW matrix is selected
	glLoadIdentity();                                    // Load the identity matrix

	glMultMatrixf(&(getTransformationMatrix()[0]));
}

algebra::Matrix4f FreeCamera::getTransformationMatrix()
{
	algebra::Vector3f rightvec = normalize(m_viewDir ^ m_up);
	algebra::Vector3f upvec = normalize(rightvec ^ m_viewDir);

	algebra::Matrix4f transform;
	for(int i=0;i<3;++i)
	{
		transform.at(i,0) = rightvec.at(0,i);
		transform.at(i,1) = upvec.at(0,i);
		transform.at(i,2) = -m_viewDir.at(0,i);
	}
	transform.at(3,3) = 1.0;

	return transform * geom::createTranslationMatrix(-m_position[0],-m_position[1],-m_position[2]);
}


geom::Frustum FreeCamera::getFrustum() const
{
	geom::Point3D pos(&(m_position[0]));
	geom::Vector3D viewDir(&(m_viewDir[0]));
	return geom::Frustum(pos,viewDir,FREECAMERA_NEAR_DISTANCE, FREECAMERA_FAR_DISTANCE,m_FOV,m_ratio);
}
